/*
    <one line to give the library's name and an idea of what it does.>
    Copyright (C) 2013  <copyright holder> <email>

    This library is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public
    License as published by the Free Software Foundation; either
    version 2.1 of the License, or (at your option) any later version.

    This library is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
    Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public
    License along with this library; if not, write to the Free Software
    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
*/


#include "OCTImageProcessing.h"

using namespace std;

OCTImageProcessing::OCTImageProcessing()
{

}

OCTImageProcessing::~OCTImageProcessing()
{

}

Mat OCTImageProcessing::surfaceSubtraction(Mat src)
{
  Mat planeParameter =  fittingPlane(src);
  if (DEBUG)
    cout << "Plane fitting result : " << planeParameter << endl;
  
  Mat rtn = Mat::zeros(src.size(), CV_32FC1);
  // For a Mat, the cols is the image x axis, the rows is the image y axis
  for (int i = 0; i < rtn.cols; i++)
    for (int j = 0; j < rtn.rows; j++)
    {
      // return = surface - ax - by - c
      rtn.at<float>(j,i) = src.at<float>(j,i) - planeParameter.at<float>(0,0) * i 
			    - planeParameter.at<float>(1,0) * j 
			    - planeParameter.at<float>(2,0);
    }
  
  return rtn;
  
  
}


Mat OCTImageProcessing::fittingPlane(Mat src)
{
  // Total number of points
  //cout<< "Fitting plane" << endl;
  int pCount =  src.cols * src.rows;
  
  /// Least square fitting plane: ax + by + c = z
  /// 
  /// A*m = v: m is (a, b, c), v is (z)
  /// AH*A*m = AH*v
  /// ASquare * m = AH_Mul_v
  /// ASquare^(-1) * ASquare * m = ASquare^(-1) * AH_Mul_v
  /// m = Asquare^1 * AH_Mul_v
  
  Mat A(pCount, 3, CV_32FC1);
//   Mat AH(3, pCount, CV_32FC1); 
//   
//   // A * AH = ASquare
//   Mat ASquare(3, 3, CV_32FC1);
//   Mat ASquareInvert(3, 3, CV_32FC1);
//   Mat m(3, 1, CV_32FC1);
  
  Mat v(pCount, 1, CV_32FC1);
//   Mat AH_Mul_v(3, 1, CV_32FC1);
  
  int count = 0;
  
  for(int i = 0; i < src.cols; i++)
    for(int j = 0; j < src.rows; j++) {
      // Create the matrix A and v
      A.at<float>(i*src.cols + j,0) = i;
      A.at<float>(i*src.cols + j,1) = j;
      A.at<float>(i*src.cols + j,2) = 1;
      
      v.at<float>(i*src.cols + j,0) = src.at<float>(j,i); 
      count++;
    } 
  
  
  Mat planeParameter;
  solve(A, v, planeParameter, DECOMP_SVD|DECOMP_NORMAL);
  
  return planeParameter;
}

Mat OCTImageProcessing::convertSurfaceToGrayImage(Mat src)
{
  /// Convert a matrix to a gray scale image according to each cell value of the matrix

  Mat rtn;
  normalize(src, rtn, 0, 255, NORM_MINMAX, CV_8UC1);
        
  return rtn;
}

uchar OCTImageProcessing::calculateGrayValue(float input, float para1, float para2)
{
  return (uchar) (para1 * input + para2);
}

Mat OCTImageProcessing::sobelFilter(Mat src, int dx, int dy, int ksize)
{
  Mat rtn;
  
  int ddepth = CV_32FC1;
  int scale = 1;
  int delta = 0;
  
  Sobel(src, rtn, ddepth, dx, dy, ksize, scale, delta);
  
  return rtn;
}

Mat OCTImageProcessing::laplaceFilter(Mat src, int ksize)
{
  Mat rtn;
  
  int ddepth = CV_32FC1;
  int scale = 1;
  int delta = 0;
  
  Laplacian(src, rtn, ddepth, ksize, scale, delta);

  return rtn;
}

Mat OCTImageProcessing::GaussianFilter(Mat src, int ksize)
{
  Mat rtn;
  
  //GaussianBlur(src, rtn, ksize, 0, 0);
  
  return rtn;
}



